//
// Created by 29787 on 2024/5/31.
//
#include "Task scheduling_l.h"
#include <stdio.h>
#include "vesc.h"
#include "fdcan.h"
#include "SlowStart.h"
#include "tim.h"
#include "stdio.h"
#include "usart.h"
#include "stm32h7xx_hal_uart.h"
#include "AirCylinder.h"
#include "Dji.h"
#include "Dji_Init.h"
#include "UPRE_Task_Flow.h"

uint8_t last_receive_ball;
uint8_t receive_mode;
#define front_receive 1
#define belly_receive 2
extern bool once_togo;
bool can_execute;
bool flag_once_eight_grain=false;
uint8_t recbuff[100];    //һ��Ҫ����Ϊȫ�ֱ���
uint8_t len;
extern float vofa_trans[32];
extern uint8_t vofa_trans_[132];
#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif

PUTCHAR_PROTOTYPE {
    HAL_UART_Transmit_DMA(&huart7, (uint8_t *) &ch, 1);
    return ch;
}


#define null 0
extern float vofa_trans[32];
bool once_true_begin_game = true;

uint8_t fetct_num = 1;
#define ferct_num_1 1
#define ferct_num_2 2
#define ferct_num_3 3

uint8_t lay_num   = 1;
#define lay_num_1   1
#define lay_num_2   2
#define lay_num_3   3
#define lay_num_4   4
#define lay_num_5   5
#define lay_num_6   6

Motor_Vel_Pos Motor_param_set = {
        .shoot_ball_front_vesc_vel=500
};


Time_Stamp Time_stamp = {
        .flag_is_Eliminate_speedacce_jitter=true,
        .flag_is_Eliminate_speeddcce_jitter=true,
};

Main_control Main_Control = {
        .control_mode=control_mode_auto,
};

Erupt_ Erupt = {

};

Air_Param Air_para = {
        .air_cylinder_updown_catch_grain=close_AC,
};

void Air_Cylinder_Control_blue() {
    if (Air_para.air_cylinder_stretch_claw_1_l)
        airCylinder_open(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_1_l);
    else
        airCylinder_close(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_1_l);

    if (Air_para.air_cylinder_stretch_claw_1_r)
        airCylinder_open(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_1_r);
    else
        airCylinder_close(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_1_r);

    if (Air_para.air_cylinder_stretch_claw_2)
        airCylinder_open(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_2);
    else
        airCylinder_close(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_2);
    if (Air_para.air_cylinder_stretch_claw_3)
        airCylinder_open(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_3);
    else
        airCylinder_close(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_3);
    if (Air_para.air_cylinder_updown_catch_grain)
        airCylinder_open(&hfdcan2, id_ari_board, air_cylinder_updown_catch_grain);
    else
        airCylinder_close(&hfdcan2, id_ari_board, air_cylinder_updown_catch_grain);
}

void Air_Cylinder_Control_red() {
    if (Air_para.air_cylinder_stretch_claw_1_r)
        airCylinder_open(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_1_l);
    else
        airCylinder_close(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_1_l);

    if (Air_para.air_cylinder_stretch_claw_1_l)
        airCylinder_open(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_1_r);
    else
        airCylinder_close(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_1_r);
    if (Air_para.air_cylinder_stretch_claw_2)
        airCylinder_open(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_2);
    else
        airCylinder_close(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_2);
    if (Air_para.air_cylinder_stretch_claw_3)
        airCylinder_open(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_3);
    else
        airCylinder_close(&hfdcan2,id_ari_board, air_cylinder_stretch_claw_3);
    if (Air_para.air_cylinder_updown_catch_grain)
        airCylinder_open(&hfdcan2, id_ari_board, air_cylinder_updown_catch_grain);
    else
        airCylinder_close(&hfdcan2, id_ari_board, air_cylinder_updown_catch_grain);
}

void Task_change() {     ///all is single thread, so can use wait
    if(TaskFlag.Flag_ManualMode)
        Main_Control.task_state_handle=control_mode_handle;
    if(TaskFlag.Flag_ManualGetSeding)
    {
        can_execute=true;
        printf("Flag_ManualGetSeding\r\n");
        TaskFlag.Flag_ManualGetSeding=false;
    }

    if (Main_Control.control_mode == control_mode_auto) {
        if (TaskFlag.Flag_ExchangePath && flag_once_eight_grain == false) {
            lay_num = 3;
            fetct_num = 2;
            flag_once_eight_grain = true;
        }
        switch (Main_Control.task_state) {
            case game_begin: {//稳定无法提速
                osDelay(500);
                Air_para.air_cylinder_updown_catch_grain = open_AC;
                Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_fetch_l;
                Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_fetch_r;
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init;
                Motor_param_set.claw_pick_ball_grasp_dji2006_pos = claw_pick_ball_grasp_dji2006_pos_open;
                Main_Control.task_state = free_time;
            }
                break;

            case claw_grain: {
                switch (fetct_num) {
                    case ferct_num_1: {
                        Wait(800);
                        Air_para.air_cylinder_stretch_claw_1_l = open_AC;
                        Air_para.air_cylinder_stretch_claw_1_r = open_AC;
                        Wait(300);
                        Air_para.air_cylinder_updown_catch_grain = close_AC;
                        Wait(300);
                        Main_Control.task_state = free_time;
                        once_togo = true;
                        Dji_Setparam(&Dji_Control_can2_0x203, 20, 0, 0, 5000, 0, 0);
                        Dji_Setparam(&Dji_Control_can2_0x204, 20, 0, 0, 5000, 0, 0);
                        Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_lay_l;  ///
                        Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_lay_r;
                        Wait(700);
                        Dji_Setparam(&Dji_Control_can2_0x203, 20, 0, 0, 25000, 0, 0);
                        Dji_Setparam(&Dji_Control_can2_0x204, 20, 0, 0, 25000, 0, 0);
                        Wait(200);
                        Air_para.air_cylinder_stretch_claw_3 = open_AC;    ///claw1 connect claw3
                        Air_para.air_cylinder_stretch_claw_2 = open_AC;
                        osDelay(300);
                        Air_para.air_cylinder_stretch_claw_2 = close_AC;
                        Air_para.air_cylinder_stretch_claw_1_l = close_AC;
                        Air_para.air_cylinder_stretch_claw_1_r = close_AC;
                        Wait(200);
                        Motor_param_set.claw_storage_left_dji2006_pos = claw_storage_dji2006_pos_lay_l;
                        Motor_param_set.claw_storage_right_dji2006_pos = claw_storage_dji2006_pos_lay_r;
                        osDelay(100);
                        Dji_Setparam(&Dji_Control_can2_0x203, 20, 0, 0, 5000, 0, 0);
                        Dji_Setparam(&Dji_Control_can2_0x204, 20, 0, 0, 5000, 0, 0);
                        Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_fetch_l;
                        Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_fetch_r;
                        Air_para.air_cylinder_updown_catch_grain = open_AC;
                        fetct_num = ferct_num_2;
                    }
                        break;

                    case ferct_num_2: {
                        Dji_Setparam(&Dji_Control_can2_0x203, 20, 0, 0, 25000, 0, 0);
                        Dji_Setparam(&Dji_Control_can2_0x204, 20, 0, 0, 25000, 0, 0);
                        Wait(600);
                        Air_para.air_cylinder_stretch_claw_1_l = open_AC;
                        Air_para.air_cylinder_stretch_claw_1_r = open_AC;
                        Wait(300);//5
                        Air_para.air_cylinder_updown_catch_grain = close_AC;
                        osDelay(300);
                        Main_Control.task_state = free_time;
                        once_togo = true;
                        Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_lay_l;
                        Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_lay_r;
                        Wait(1100);//3
                        Air_para.air_cylinder_stretch_claw_2 = open_AC;    ///claw1 connect claw3
                        osDelay(200);
                        Air_para.air_cylinder_stretch_claw_1_l = close_AC;
                        Air_para.air_cylinder_stretch_claw_1_r = close_AC;
                        Wait(200);
                        Air_para.air_cylinder_updown_catch_grain = open_AC;
                        Dji_Setparam(&Dji_Control_can2_0x203, 20, 0, 0, 5000, 0, 0);
                        Dji_Setparam(&Dji_Control_can2_0x204, 20, 0, 0, 5000, 0, 0);
                        Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_fetch_l;
                        Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_fetch_r;
                        fetct_num = ferct_num_3;
                    }
                        break;

                    case ferct_num_3: {
                        Dji_Setparam(&Dji_Control_can2_0x203, 20, 0, 0, 25000, 0, 0);
                        Dji_Setparam(&Dji_Control_can2_0x204, 20, 0, 0, 25000, 0, 0);
                        Wait(600);
                        Air_para.air_cylinder_stretch_claw_1_l = open_AC;
                        Air_para.air_cylinder_stretch_claw_1_r = open_AC;
                        Wait(300);
                        Air_para.air_cylinder_updown_catch_grain = close_AC;
                        Wait(200);
                        Main_Control.task_state = free_time;
                        once_togo = true;

                        Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_fetch_lay_l;
                        Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_fetch_lay_r;

                    }
                        break;
                }
            }
                break;

            case lay_grain: {
                switch (lay_num) {
                    case lay_num_1: {
                        Wait(300);
                        Air_para.air_cylinder_updown_catch_grain = open_AC;
                        Wait(600);
                        Air_para.air_cylinder_stretch_claw_1_r = close_AC;
                        Wait(100);
                        Air_para.air_cylinder_updown_catch_grain = close_AC;
                        Main_Control.task_state = free_time;
                        once_togo = true;

                        lay_num = lay_num_2;
                    }
                        break;

                    case lay_num_2: {
                        Wait(300);
                        Air_para.air_cylinder_updown_catch_grain = open_AC;
                        Wait(600);
                        Air_para.air_cylinder_stretch_claw_1_l = close_AC;
                        Wait(100);
                        Air_para.air_cylinder_updown_catch_grain = close_AC;
                        once_togo = true;
                        Main_Control.task_state = free_time;
                        Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_lay_l;
                        Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_lay_r;
                        Wait(700);//5
                        Air_para.air_cylinder_stretch_claw_1_l = open_AC;
                        Air_para.air_cylinder_stretch_claw_1_r = open_AC;
                        osDelay(200);
                        Air_para.air_cylinder_stretch_claw_2 = close_AC;
                        Wait(200);
                        Dji_Setparam(&Dji_Control_can2_0x203, 20, 0, 0, 5000, 0, 0);
                        Dji_Setparam(&Dji_Control_can2_0x204, 20, 0, 0, 5000, 0, 0);
                        Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_fetch_lay_l;
                        Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_fetch_lay_r;
                        Wait(200);
                        Motor_param_set.claw_storage_left_dji2006_pos = claw_storage_dji2006_pos_fetch_l;
                        Motor_param_set.claw_storage_right_dji2006_pos = claw_storage_dji2006_pos_fetch_r;
                        lay_num = lay_num_3;
                        Wait(800);
                        Air_para.air_cylinder_stretch_claw_2 = open_AC;
                    }
                        break;

                    case lay_num_3: {
                        Dji_Setparam(&Dji_Control_can2_0x203, 20, 0, 0, 25000, 0, 0);
                        Dji_Setparam(&Dji_Control_can2_0x204, 20, 0, 0, 25000, 0, 0);
                        Wait(300);
                        Air_para.air_cylinder_updown_catch_grain = open_AC;
                        Wait(600);
                        Air_para.air_cylinder_stretch_claw_1_l = close_AC;
                        Wait(100);
                        Air_para.air_cylinder_updown_catch_grain = close_AC;
                        Main_Control.task_state = free_time;
                        once_togo = true;

                        lay_num = lay_num_4;
                    }
                        break;

                    case lay_num_4: {
                        Wait(300);
                        Air_para.air_cylinder_updown_catch_grain = open_AC;
                        Wait(600);
                        Air_para.air_cylinder_stretch_claw_1_r = close_AC;
                        Wait(100);
                        Air_para.air_cylinder_updown_catch_grain = close_AC;
                        Main_Control.task_state = free_time;
                        once_togo = true;
                        Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_lay_l;
                        Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_lay_r;
                        Wait(1000);

                        Air_para.air_cylinder_stretch_claw_1_l = open_AC;
                        Air_para.air_cylinder_stretch_claw_1_r = open_AC;
                        Wait(300);
                        Air_para.air_cylinder_stretch_claw_3 = close_AC;
                        Air_para.air_cylinder_stretch_claw_2 = close_AC;
                        Wait(200);
                        Air_para.air_cylinder_updown_catch_grain = close_AC;
                        Dji_Setparam(&Dji_Control_can2_0x203, 20, 0, 0, 5000, 0, 0);
                        Dji_Setparam(&Dji_Control_can2_0x204, 20, 0, 0, 5000, 0, 0);
                        Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_fetch_lay_l;
                        Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_fetch_lay_r;
                        Wait(200);
                        lay_num = lay_num_5;
                    }
                        break;

                    case lay_num_5: {
                        Dji_Setparam(&Dji_Control_can2_0x203, 20, 0, 0, 25000, 0, 0);
                        Dji_Setparam(&Dji_Control_can2_0x204, 20, 0, 0, 25000, 0, 0);
                        Wait(300);
                        Air_para.air_cylinder_updown_catch_grain = open_AC;
                        Wait(600);
                        Air_para.air_cylinder_stretch_claw_1_r = close_AC;
                        Wait(100);
                        Air_para.air_cylinder_updown_catch_grain = close_AC;
                        Main_Control.task_state = free_time;

                        once_togo = true;

                        lay_num = lay_num_6;
                    }
                        break;

                    case lay_num_6: {
                        Wait(300);
                        Air_para.air_cylinder_updown_catch_grain = open_AC;
                        Wait(600);
                        Air_para.air_cylinder_stretch_claw_1_l = close_AC;
                        osDelay(100);
                        Air_para.air_cylinder_updown_catch_grain = close_AC;
                        Main_Control.task_state = free_time;

                        once_togo = true;
                        osDelay(500);

                        Motor_param_set.claw_grain_left_dji3508_pos = claw_grain_dji3508_pos_lay_l;
                        Motor_param_set.claw_grain_right_dji3508_pos = claw_grain_dji3508_pos_lay_r;
                    }
                        break;
                }

            }
                break;


            case belly_receball: {
                receive_mode = belly_receive;
                Dji_Setparam(&Dji_Control_can1_0x202,20,0,0,50000,0,0);
                Motor_param_set.claw_pick_ball_grasp_dji2006_pos = claw_pick_ball_grasp_dji2006_pos_open;
                Wait(500);
                Dji_Setparam(&Dji_Control_can1_0x203, 15, 0, 0, 20000, 0, 0);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_belly;
                Wait(1000);
                Dji_Setparam(&Dji_Control_can1_0x202,15,0,0,5000,0,0);
                Motor_param_set.claw_pick_ball_grasp_dji2006_pos = claw_pick_ball_grasp_dji2006_pos_close + 15;
                Wait(2000);
                Dji_Setparam(&Dji_Control_can1_0x203, 15, 0, 0, 6000, 0, 0);
                Motor_param_set.shoot_ball_behind_dji2006_vel = 2500;
                //                Motor_param_set.shoot_ball_front_vesc_vel=1000;
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 26;
                Wait(1800 );
                Motor_param_set.claw_pick_ball_grasp_dji2006_pos = claw_pick_ball_grasp_dji2006_pos_close + 20;
                Wait(400);
                Dji_Setparam(&Dji_Control_can1_0x203, 20, 0, 0, 50000, 0, 0);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 110;
                Motor_param_set.shoot_ball_behind_dji2006_vel = -2500;
                Wait(500);
                Motor_param_set.shoot_ball_behind_dji2006_vel = 0;
                Motor_param_set.shoot_ball_front_vesc_vel=500;
                Main_Control.task_state = free_time;
            }
            break;
            case front_receball: {
                receive_mode = front_receive;
                Dji_Setparam(&Dji_Control_can1_0x203, 15, 0, 0, 20000, 0, 0);
                Dji_Setparam(&Dji_Control_can1_0x202,20,0,0,50000,0,0);
                Motor_param_set.claw_pick_ball_grasp_dji2006_pos = claw_pick_ball_grasp_dji2006_pos_open;
                Wait(250);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_front;
                Wait(2000);
                Dji_Setparam(&Dji_Control_can1_0x203, 15, 0, 0, 20000, 0, 0);
                Motor_param_set.claw_pick_ball_grasp_dji2006_pos = claw_pick_ball_grasp_dji2006_pos_close;
                Wait(1500);
                Motor_param_set.shoot_ball_front_vesc_vel = -3000;
                Dji_Setparam(&Dji_Control_can1_0x203, 15, 0, 0, 8000, 0, 0);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init + 10;
                Wait(800);
                Motor_param_set.claw_pick_ball_grasp_dji2006_pos = claw_pick_ball_grasp_dji2006_pos_close + 10;
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 10;
                osDelay(80);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 20;
                osDelay(80);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 30;
                osDelay(80);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 40;
                osDelay(80);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 50;
                osDelay(80);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 60;
                osDelay(80);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 70;
                osDelay(80);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 80;
                osDelay(80);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 90;
                osDelay(80);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init - 110;
                osDelay(300);
                Motor_param_set.claw_pick_ball_grasp_dji2006_pos = claw_pick_ball_grasp_dji2006_pos_close;
                Motor_param_set.shoot_ball_behind_dji2006_vel = 0;
                Motor_param_set.shoot_ball_front_vesc_vel = 500;
                Main_Control.task_state = free_time;

            }
                break;
            case accelerate_shoot: {
                Motor_param_set.shoot_ball_front_vesc_vel += shoot_ball_front_vesc_vel_change;
                Wait(10);
                if (receive_mode == belly_receive)
                    Motor_param_set.shoot_ball_behind_dji2006_vel = shoot_ball_behind_dji2006_vel_shoot_belly;
//
                if (receive_mode == front_receive)
                    Motor_param_set.shoot_ball_behind_dji2006_vel = shoot_ball_behind_dji2006_vel_shoot;
                Main_Control.task_state = free_time;
            }
                break;


            case decelerate_shoot: {
                Motor_param_set.shoot_ball_front_vesc_vel -= shoot_ball_front_vesc_vel_change;
                Wait(10);
                Main_Control.task_state = free_time;
            }
                break;

            case shoot_ball: {
                Motor_param_set.claw_pick_ball_grasp_dji2006_pos = claw_pick_ball_grasp_dji2006_pos_close;
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init + 20;
                osDelay(50);
                Wait(1000);
                Main_Control.task_state = free_time;
//            once_togo=true;

                Wait(1000);
                Motor_param_set.claw_pick_ball_overturn_dji2006_pos = claw_pick_ball_overturn_dji2006_pos_init;;
                Motor_param_set.shoot_ball_behind_dji2006_vel = 0;
                Motor_param_set.shoot_ball_front_vesc_vel = 500;
            }
                break;
        }
    }

    if(Main_Control.control_mode==control_mode_handle)
    {
        switch (Main_Control.task_state_handle && can_execute) {
            case task_state_handle_step1:{     ///恢复抓苗状态
                Motor_param_set.claw_grain_left_dji3508_pos=claw_grain_dji3508_pos_fetch_l;
                Motor_param_set.claw_grain_right_dji3508_pos=claw_grain_dji3508_pos_fetch_r;
                Air_para.air_cylinder_updown_catch_grain=open_AC;
                Air_para.air_cylinder_stretch_claw_1_l=close_AC;
                Air_para.air_cylinder_stretch_claw_1_r=close_AC;
                Main_Control.task_state_handle=task_state_handle_step2;
                can_execute= false;
                Wait(30);
            }

            case task_state_handle_step2:{    ///取苗.

                Motor_param_set.claw_grain_left_dji3508_pos=claw_grain_dji3508_pos_fetch_l;
                Motor_param_set.claw_grain_right_dji3508_pos=claw_grain_dji3508_pos_fetch_r;
                Air_para.air_cylinder_stretch_claw_1_l=open_AC;
                Air_para.air_cylinder_stretch_claw_1_r=open_AC;
                Wait(500);
                Air_para.air_cylinder_updown_catch_grain=close_AC;
                Main_Control.task_state_handle=task_state_handle_step3;
                can_execute= false;
                Wait(30);
            }

            case task_state_handle_step3:{    ///左放苗.
                Motor_param_set.claw_grain_left_dji3508_pos=claw_grain_dji3508_pos_fetch_l;
                Motor_param_set.claw_grain_right_dji3508_pos=claw_grain_dji3508_pos_fetch_r;
                Air_para.air_cylinder_updown_catch_grain=open_AC;
                Wait(400);
                Air_para.air_cylinder_stretch_claw_1_l=close_AC;
                Wait(400);
                Air_para.air_cylinder_updown_catch_grain=close_AC;
                Main_Control.task_state_handle=task_state_handle_step4;
                can_execute= false;
                Wait(30);
            }

            case task_state_handle_step4:{    ///右放苗.
                Motor_param_set.claw_grain_left_dji3508_pos=claw_grain_dji3508_pos_fetch_l;
                Motor_param_set.claw_grain_right_dji3508_pos=claw_grain_dji3508_pos_fetch_r;
                Air_para.air_cylinder_updown_catch_grain=open_AC;
                Wait(400);
                Air_para.air_cylinder_stretch_claw_1_r=close_AC;
                Wait(400);
                Air_para.air_cylinder_updown_catch_grain=close_AC;
                Main_Control.task_state_handle=task_state_handle_step1;
                can_execute= false;
                Wait(30);
            }
        }
    }

}

void time_stamp() {

}

void serial_init()
{
    HAL_UART_Receive_DMA(&huart7, recbuff, 100);  //ʹ���µĽ���
    __HAL_UART_ENABLE_IT(&huart7, UART_IT_IDLE);  //���������ж�        д���������У�ѭ���⼴��     д���������У�ѭ���⼴��
}

void windows_cb()
{
    if(__HAL_UART_GET_FLAG(&huart7,UART_FLAG_IDLE)!=RESET)  //��������˿����ж�
    {
        __HAL_UART_CLEAR_IDLEFLAG(&huart7);  ///����жϱ�־λ
        HAL_UART_DMAStop(&huart7);   ///ֹͣ����
        HAL_UART_Receive_DMA(&huart7,recbuff,100);  ///ʹ���µĽ���
        sscanf((const char *) recbuff, "%d", &Main_Control.task_state);
//        printf("***************************************%d\r\n",Main_Control.task_state);
    }
}



